"""  
需求：
流程：
    1.导包；
    2.初始化ROS2客户端；
    3.自定义节点类；
                  
    4.调用spain函数，并传入节点对象；
    5.资源释放。


"""
#1.导包；
import rclpy
from rclpy.node import Node
from tf2_ros.transform_broadcaster import TransformBroadcaster
from turtlesim.msg import Pose
from geometry_msgs.msg import TransformStamped
import tf_transformations

#3.自定义节点类；
class Exer02TFBroadcasterPy(Node):
    def __init__(self):
        super().__init__("exer02_tf_broadcaster_py_node")
        self.declare_parameter("turtle","turtle1")
        self.turtle=self.get_parameter("turtle").get_parameter_value().string_value
        self.broadcaster=TransformBroadcaster(self)
        self.sub=self.create_subscription(
            Pose,
            "/"+self.turtle+"/pose",#这种写法可以在launch文件传参，启动turtle1，turtle2与world的相对坐标
            self.do_pose,
            10
        )
    def do_pose(self,pose):
        ts=TransformStamped()
        ts.header.stamp=self.get_clock().now().to_msg()
        ts.header.frame_id="world"
        ts.child_frame_id=self.turtle

        ts.transform.translation.x=pose.x
        ts.transform.translation.y=pose.y
        ts.transform.translation.z=0.0

        qtn=tf_transformations.quaternion_from_euler(0.0,0.0,pose.theta)
        ts.transform.rotation.x=qtn[0]
        ts.transform.rotation.y=qtn[1]
        ts.transform.rotation.z=qtn[2]
        ts.transform.rotation.w=qtn[3]

        self.broadcaster.sendTransform(ts)
def main():
    #2.初始化ROS2客户端；
    rclpy.init()
    #4.调用spain函数，并传入节点对象；
    rclpy.spin(Exer02TFBroadcasterPy())
    #5.资源释放。
    rclpy.shutdown()

if __name__=='__main__':
    main() 